#include "StdAfx.h"
#include "Trajectory.h"
#include "RadaParam.h"

CTrajectory::CTrajectory(LPCTSTR stzEphemFile, double dParam)
: m_ephem(stzEphemFile)
, m_dStep(10.0)
{
	CRadaParam param;
	m_pRada = auto_ptr<Everhardt>( new Everhardt( param.m_NV, param.m_NCLASS, param.m_NOR, param.m_NI, param.m_LL, param.m_XL) );
	m_dStep = param.m_dIntegrationStep;

	vector<eBodies> vecBodyesNum;
	vecBodyesNum.push_back(eEARTH);
	//vecBodyesNum.push_back(eMOON);

	m_pAppSearcher = auto_ptr<CApproachSearcher>( new CApproachSearcher(this, &m_ephem, vecBodyesNum, dParam));
}

CTrajectory::~CTrajectory()
{

}

ListApproachPtr CTrajectory::Move(double dStartTime, double dEndTime, Body &body, TimePosTrace * pTrace)
{
	ListApproachPtr listApproaches;
	if(pTrace)
	{
		TimePosTrace & trace = *pTrace;
		TrajTrace::Ptr pos(new TrajTrace(body));
		trace[dStartTime] = pos;
	}

	while(dStartTime != dEndTime)
	{
		double dCurStep = m_dStep;
		double dTime = dEndTime - dStartTime;

		if (fabs(dTime) < dCurStep)
			dCurStep = fabs(dTime);

		if( dTime < 0.0)
			dCurStep = -dCurStep;

		ListApproachPtr curList = m_pAppSearcher->Check(dStartTime, body);
		listApproaches.insert(listApproaches.end(), curList.begin(), curList.end());

		if(m_pRada.get() == NULL)
			throw CCustomException(_T("Empty Rada"));
		m_pRada->rada27(body.x, body.v, dStartTime, dStartTime+dCurStep, this);
		dStartTime += dCurStep;
		if(pTrace)
		{
			TimePosTrace & trace = *pTrace;
			TrajTrace::Ptr pos( new TrajTrace(body) );
			trace[dStartTime] = pos;
		}
		//printf("%lf %s | %lf %lf\n", dStartTime, CEphemWrapper::StrDateFromJD(dStartTime).c_str(), body.x[0], body.x[1]);
	}
	return listApproaches;
}

bool CTrajectory::SimpleMove(double start, double end, Body &body)
{
	if(m_pRada.get() == NULL)
		throw CCustomException(_T("Empty Rada"));

	m_pRada->rada27(body.x, body.v, start, end, this);
	return true;
}

bool CTrajectory::Force(int nv, double time, double *x, double *f)
{
	double d[3];

	if(nv != 3)
		throw CCustomException(_T("nv must be 3"));
	
	AllBodies bodies;
	m_ephem.GetCoord(time, true, bodies, false);

	for(int i = 0; i<nv; i++)
	{
		f[i] = 0.0;
	}

	for(int body = 0; body < sizeof(bodies.bodies)/sizeof(bodies.bodies[0]); ++body)
	{
		double rr = 0.0;
		const Body & CurBody = bodies.bodies[body];
		for(int i = 0; i < nv; i++)
		{
			d[i] = CurBody.x[i] - x[i];
			rr += d[i] * d[i];
		}
		double r = sqrt(rr);
		double rrr = r * rr;
		double rrr1 = 1.0/rrr;
		for(int i = 0; i < nv; i++)
		{
			f[i] += CurBody.GM * d[i] * rrr1;
		}
	}
	return true;
}